#ifndef H_GYRO
#define H_GYRO

#include "Arduino.h"




class Gyro
{
private:
  byte state;
  int count;
  float rs,ps,ys;
#if defined(ITG3200)
  const static byte 
    G32_SMPLRT_DIV=0x15, G32_DLPF_FS=0x16, G32_INT_CFG=0x17, G32_PWR_MGM=0x3E, 
#elif defined(L3G4200D)
  const static byte 
    CTRL_REG1=0x20, CTRL_REG5=0x24;
#endif

public:
  int errorCount;
  byte errorCode;

  float r0,p0,y0; // TODO remove from public 
  int ri,pi,yi,temp;
  float r,p,y;

  byte setup()
  {
    byte ret;


#if defined(ITG3200)
    ret=fw.write(GYRO_ADDR, G32_PWR_MGM, 0x80); // force reset internal osc
    if (ret!=0) return 1;

    ret=fw.write(GYRO_ADDR, G32_SMPLRT_DIV, 0x00); // 1khz/(1+SMPLRT_DIV)
    if (ret!=0) return 2;

    ret=fw.write(GYRO_ADDR, G32_DLPF_FS, 0x18 | 0); // +/- 2000 dgrs/sec, 1KHz, 42hz filter
    if (ret!=0) return 3;
#elif defined(L3G4200D)
    ret=fw.write(GYRO_ADDR, CTRL_REG1, 0x9F); // 400Hz ODR, 25hz filter, run!
    if (ret!=0) return 1;

    ret=fw.write(GYRO_ADDR, CTRL_REG5, 0x02); // low pass filter enable
    if (ret!=0) return 2;
#elif defined(MPU6050)
    ret=fw.write(GYRO_ADDR, 0x6b, 0x80);
    delay(5);
    if (ret!=0) return 1;
    ret=fw.write(GYRO_ADDR, 0x19, 0x00);
    if (ret!=0) return 2;
    ret=fw.write(GYRO_ADDR, 0x1a, 0x00);
    if (ret!=0) return 3;
    ret=fw.write(GYRO_ADDR, 0x6b, 0x03);
    if (ret!=0) return 4;
    ret=fw.write(GYRO_ADDR, 0x1b, 0x18);
    if (ret!=0) return 5;
#endif

    return 0;
  }

  void setState(byte s)
  {
    state=s;
    if (s==1) 
    {
      count=0;
      rs=0;
      ps=0;
      ys=0;
    }
  }

  void save(void)
  {
    para.writeFloat(r0);
    para.writeFloat(p0);
    para.writeFloat(y0);
  }

  void load(void)
  {
    r0=para.readFloat();
    p0=para.readFloat();
    y0=para.readFloat();
  }

  byte update()
  {
    byte ret;
    byte buff[4*sizeof(int)];

#if defined(ITG3200)
    ret=fw.readBuf(GYRO_ADDR, 0x1b, buff, 4*sizeof(int)); //read the gyro data from the ITG3200

    if (ret==0)
    {
      temp = (buff[0] << 8) | buff[1];
      //temp=35+(temp+13200)/280; // TODO Not used
      GYRO_ORIENTATION( 
      ((buff[4] << 8) | buff[5]), 
      -((buff[2] << 8) | buff[3]), 
      ((buff[6] << 8) | buff[7]) );
    }
#elif defined(L3G4200D)
    ret=fw.readBuf(GYRO_ADDR, 0x80 | 0x28, buff, 3*sizeof(int)); //read the gyro data with AUTO increment 0x80

    if (ret==0)
    {
      GYRO_ORIENTATION( 
      ((buff[1] << 8) | buff[0]), 
      ((buff[3] << 8) | buff[2]), 
      ((buff[5] << 8) | buff[4]) );
    }
#elif defined(MPU6050)
    ret=fw.readBuf(GYRO_ADDR, 0x43, buff, 3*sizeof(int)); //read the gyro data with AUTO increment 0x80

    if (ret==0)
    {
      GYRO_ORIENTATION( 
      ((buff[0] << 8) | buff[1]), 
      ((buff[2] << 8) | buff[3]), 
      ((buff[4] << 8) | buff[5]) );
    }
#endif  
    else
    {
      fw.reset();
      errorCount++;
      errorCode=ret;
      ri=r0;
      pi=p0;
      yi=y0;
    }



    if (state==1) // 1=search zero
    {
      rs+=ri;
      ps+=pi;
      ys+=yi;
      count++;
      if (count==2000)
      {
        r0=rs/2000.0;
        p0=ps/2000.0;
        y0=ys/2000.0;
        state=0;
        saveAll();
        led.setOutput(0);
      }
    }

#if defined(ITG3200)
    r=(ri-r0)*M_PI/(180.0*14.375);
    p=(pi-p0)*M_PI/(180.0*14.375);
    y=(yi-y0)*M_PI/(180.0*14.375);
#elif defined(L3G4200D)
    r=(ri-r0)*M_PI*8.75/180000.0;
    p=(pi-p0)*M_PI*8.75/180000.0;
    y=(yi-y0)*M_PI*8.75/180000.0;
#elif defined(MPU6050)
    r=(ri-r0)*M_PI/(180.0*16.4);
    p=(pi-p0)*M_PI/(180.0*16.4);
    y=(yi-y0)*M_PI/(180.0*16.4);
#endif

    return ret;
  }

} 
gyro;

#endif





























